#include "Kinectv3.h"
#include <unistd.h>

 
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <opencv2/opencv.hpp>
#include <string>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/visualization/cloud_viewer.h>


hitcrt::Kinectv3 cam("斜搭.mkv"); ///创建类对象，并打开路径上的mkv视频


//#############深度图像+RGB图像 转 点云图像########################
int main(int argc, char** argv)
{

 
	std::shared_ptr<cv::Mat> depth_image;
    std::shared_ptr<cv::Mat> color_image;

	cv::Mat rgb, depth;
    std::string result;

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

	pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");//直接创造一个显示窗口
    hitcrt::Kinectv3::Point flatPoint; 
    while (!viewer.wasStopped())
	{
    result = cam.grab();

    depth_image = cam.getDepthImage(); ///获取与彩色图对齐的深度图深度图
    color_image = cam.getAlignedColorImage();   ///获取彩色图
    std::cout << "进入主函数！" << std::endl;
	for (int m = 0; m < (*color_image).rows; m++){
		for (int n = 0; n < (*color_image).cols; n++)
		{
        std::cout << m <<" "<< n << std::endl;
		// 获取深度图中(m,n)处的值
        
		ushort d = (*depth_image).ptr<ushort>(m)[n];
        std::cout <<"a"<< std::endl;
		// d 可能没有值，若如此，跳过此点
		if (d == 0)
			continue;
		// d 存在值，则向点云增加一个点
		pcl::PointXYZRGB p;
        std::cout <<"b"<< std::endl;
        cam.get3dPoint(hitcrt::Kinectv3::Point((float)n, (float)m, (float)(*depth_image).at<short>(n, m)), flatPoint);
		// 计算这个点的空间坐标
        p.x = flatPoint.x;
        p.y = flatPoint.y;
        p.z = flatPoint.z;
        std::cout <<"c"<< std::endl;
		// 从rgb图像中获取它的颜色
		// rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
		p.b = (color_image)->ptr<uchar>(n)[m * 3];
		p.g = (color_image)->ptr<uchar>(n)[m * 3 + 1];
		p.r = (color_image)->ptr<uchar>(n)[m * 3 + 2];
        std::cout <<"d"<< std::endl;
		// 把p加入到点云中
		cloud->points.push_back(p);

		}
	}
	//显示点云图

	viewer.showCloud(cloud);//再这个窗口显示点云
	cloud->points.clear();
	}
 

	// 清除数据并退出


	return 0;
}
